/* * Copyright (c) 2021 BlackWalnut Labs., Ltd.
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


#include <stdio.h>
#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_watchdog.h"
#include "iot_i2c.h"
#include "iot_io.h"
#include "iot_errno.h"
#include "icm20948.h"

const uint8_t I2C_PORT_IDX = 0;
const uint32_t I2C_BAUDRATE = 400000;

ICM_20948_Status_e serif_i2c_write(uint8_t regaddr, uint8_t *pdata, uint32_t len, void *user)
{
    if (user == NULL)
    {
        return ICM_20948_Stat_ParamErr;
    }

    uint8_t addr = ((ICM_20948 *)user)->_addr;
    uint8_t send_data[1 + len];
    send_data[0] = regaddr;
    memcpy(&send_data[1], pdata, len);

    unsigned int retval = IoTI2cWrite(I2C_PORT_IDX, (addr << 1) | 0x0, send_data, len + 1);
    return retval == IOT_SUCCESS ? ICM_20948_Stat_Ok : ICM_20948_Stat_NoData;
}

ICM_20948_Status_e serif_i2c_read(uint8_t reg, uint8_t *buff, uint32_t len, void *user)
{
    if (user == NULL)
    {
        return ICM_20948_Stat_ParamErr;
    }

    uint8_t addr = ((ICM_20948 *)user)->_addr;
    // uint8_t send_data[1] = {reg};
    unsigned int retval = IoTI2cWriteRead(I2C_PORT_IDX, (addr << 1) | 0x0, &reg, 1, buff, len);

    return retval == IOT_SUCCESS ? ICM_20948_Stat_Ok : ICM_20948_Stat_NoData;
}

void printScaledAGMT(ICM_20948 *sensor)
{
    printf("Scaled. Acc (mg) [ ");
    printf("%f", accX(sensor));
    printf(", ");
    printf("%f", accY(sensor));
    printf(", ");
    printf("%f", accZ(sensor));
    printf(" ], Gyr (DPS) [ ");
    printf("%f", gyrX(sensor));
    printf(", ");
    printf("%f", gyrY(sensor));
    printf(", ");
    printf("%f", gyrZ(sensor));
    printf(" ], Mag (uT) [ ");
    printf("%f", magX(sensor));
    printf(", ");
    printf("%f", magY(sensor));
    printf(", ");
    printf("%f", magZ(sensor));
    printf(" ], Tmp (C) [ ");
    printf("%f", temperature(sensor));
    printf(" ]");
    printf("\r\n");
}

static void ICM20948_STARTUP(void)
{
    // Initial I2C
    IoTI2cInit(I2C_PORT_IDX, I2C_BAUDRATE);
    IoTIoSetFunc(IOT_IO_NAME_10, IOT_IO_FUNC_10_I2C0_SCL);
    IoTIoSetFunc(IOT_IO_NAME_9, IOT_IO_FUNC_9_I2C0_SDA);

    ICM_20948 icm_20948;
    ICM_20948_Device_t device = {0};
    ICM_20948_Serif_t serif = {0};
    serif.write = serif_i2c_write;
    serif.read = serif_i2c_read;
    serif.user = &icm_20948;
    device._serif = &serif;
    icm_20948._addr = ICM_20948_I2C_ADDR_AD0;
    icm_20948._device = device;

    ICM_20948_Status_e retval = icm20948_begin(&icm_20948);
    printf(statusString(&icm_20948, retval));

    // Here we are doing a SW reset to make sure the device starts in a known state
    swReset(&icm_20948);

    if (icm_20948.status != ICM_20948_Stat_Ok)
    {
        printf("Software Reset returned: ");
        printf(statusString(&icm_20948, ICM_20948_Stat_NUM));
    }
    usleep(250);

    // Now wake the sensor up
    deviceSleep(&icm_20948, false);
    lowPower(&icm_20948, false);

    // The next few configuration functions accept a bit-mask of sensors for which the settings should be applied.

    // Set Gyro and Accelerometer to a particular sample mode
    // options: ICM_20948_Sample_Mode_Continuous
    //          ICM_20948_Sample_Mode_Cycled
    setSampleMode(&icm_20948, (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Continuous);
    if (icm_20948.status != ICM_20948_Stat_Ok)
    {
        printf("setSampleMode returned: ");
        printf(statusString(&icm_20948, ICM_20948_Stat_NUM));
    }

    // Set full scale ranges for both acc and gyr
    ICM_20948_fss_t myFSS; // This uses a "Full Scale Settings" structure that can contain values for all configurable sensors

    myFSS.a = gpm2; // (ICM_20948_ACCEL_CONFIG_FS_SEL_e)
                    // gpm2
                    // gpm4
                    // gpm8
                    // gpm16

    myFSS.g = dps250; // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e)
                      // dps250
                      // dps500
                      // dps1000
                      // dps2000

    setFullScale(&icm_20948, (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myFSS);
    if (icm_20948.status != ICM_20948_Stat_Ok)
    {
        printf("setFullScale returned: ");
        printf(statusString(&icm_20948, ICM_20948_Stat_NUM));
    }

    // Set up Digital Low-Pass Filter configuration
    ICM_20948_dlpcfg_t myDLPcfg;    // Similar to FSS, this uses a configuration structure for the desired sensors
    myDLPcfg.a = acc_d473bw_n499bw; // (ICM_20948_ACCEL_CONFIG_DLPCFG_e)
                                    // acc_d246bw_n265bw      - means 3db bandwidth is 246 hz and nyquist bandwidth is 265 hz
                                    // acc_d111bw4_n136bw
                                    // acc_d50bw4_n68bw8
                                    // acc_d23bw9_n34bw4
                                    // acc_d11bw5_n17bw
                                    // acc_d5bw7_n8bw3        - means 3 db bandwidth is 5.7 hz and nyquist bandwidth is 8.3 hz
                                    // acc_d473bw_n499bw

    myDLPcfg.g = gyr_d361bw4_n376bw5; // (ICM_20948_GYRO_CONFIG_1_DLPCFG_e)
                                      // gyr_d196bw6_n229bw8
                                      // gyr_d151bw8_n187bw6
                                      // gyr_d119bw5_n154bw3
                                      // gyr_d51bw2_n73bw3
                                      // gyr_d23bw9_n35bw9
                                      // gyr_d11bw6_n17bw8
                                      // gyr_d5bw7_n8bw9
                                      // gyr_d361bw4_n376bw5

    setDLPFcfg(&icm_20948, (ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myDLPcfg);
    if (icm_20948.status != ICM_20948_Stat_Ok)
    {
        printf("setDLPcfg returned: ");
        printf(statusString(&icm_20948, ICM_20948_Stat_NUM));
    }

    // Choose whether or not to use DLPF
    // Here we're also showing another way to access the status values, and that it is OK to supply individual sensor masks to these functions
    ICM_20948_Status_e accDLPEnableStat = enableDLPF(&icm_20948, ICM_20948_Internal_Acc, true);
    printf("Enable DLPF for Accelerometer returned: ");
    printf(statusString(&icm_20948, accDLPEnableStat));
    printf("\r\n");

    ICM_20948_Status_e gyrDLPEnableStat = enableDLPF(&icm_20948, ICM_20948_Internal_Gyr, true);
    printf("Enable DLPF for Gyroscope returned: ");
    printf(statusString(&icm_20948, gyrDLPEnableStat));
    printf("\r\n");

    // Choose whether or not to start the magnetometer
    startupMagnetometer(&icm_20948, false);
    if (icm_20948.status != ICM_20948_Stat_Ok)
    {
        printf("startupMagnetometer returned: ");
        printf(statusString(&icm_20948, ICM_20948_Stat_NUM));
    }

    printf("\r\n");
    printf("Configuration complete!\r\n");

    while (true)
    {
        if (dataReady(&icm_20948))
        {
            getAGMT(&icm_20948);         // The values are only updated when you call 'getAGMT'
            printScaledAGMT(&icm_20948); // This function takes into account the scale settings from when the measurement was made to calculate the values with units
            osDelay(30);
        }
        else
        {
            printf("Waiting for data\r\n");
            osDelay(500);
        }
    }
}

static void APP(void)
{
    IoTWatchDogDisable();

    osThreadAttr_t attr;

    attr.name = "ICM20948_STARTUP";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 10240;
    attr.priority = osPriorityLow1;

    if (osThreadNew(ICM20948_STARTUP, NULL, &attr) == NULL)
    {
        printf("[ICM20948_STARTUP] Falied to create ICM20948_STARTUP!\n");
    }
}

SYS_RUN(APP);